#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <unistd.h>
#include "ohos_types.h"
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_errno.h"
#include "iot_gpio.h"
#include "iot_gpio_ex.h"
#include "iot_uart.h"
#include "hi_time.h"
#include "hi_watchdog.h"
#include "hi_lowpower.h"
#include "hi_wifi_api.h"
#include "hi_tsensor.h"

#define IOT_UART     1
#define GPIO_UART_TX 0
#define GPIO_UART_RX 1
#define GPIO_LED     2
#define GPIO_IR      11
#define GPIO_SERVO0  6
#define GPIO_SERVO1  7
#define GPIO_SERVO2  9
#define GPIO_SERVO3  10
#define SERVO_NUM    4
#define LEFT_90  500
#define LEFT_45  1000
#define MID_0    1500
#define RIGHT_45 2000
#define RIGHT_90 2500
#define LID_OPEN_TIME 3
#define SLEEP_TIMEOUT 5

const char cmd_sleep[] = "s";
const char cmd_wake[] = "w";

struct ServoControl {
    unsigned int pin;
    long long time;
} servos[SERVO_NUM];

bool ir_triggered = false;

static inline void delay_us(unsigned int us) {
    hi_udelay(us); // 会阻塞 CPU
    // osDelay(us / 500); // 需要把 LOSCFG_BASE_CORE_TICK_PER_SECOND 设为 2000 (不好使)
}

static void set_angle(unsigned int pin, unsigned int duty) {
    for (int i = 0; i < 10; i++) {
        IoTGpioSetOutputVal(pin, IOT_GPIO_VALUE1);
        delay_us(duty);
        IoTGpioSetOutputVal(pin, IOT_GPIO_VALUE0);
        delay_us(20000 - duty);
    }
}

static hi_u32 hw_prepare(void) {
    if (hi_lpc_get_type() == HI_DEEP_SLEEP) {
        hi_tsensor_lp_save();
    }
    return HI_ERR_SUCCESS;
}

static hi_u32 hw_resume(void) {
    if (hi_lpc_get_type() == HI_DEEP_SLEEP) {
        hi_tsensor_lp_restore();
    }
    return HI_ERR_SUCCESS;
}

static void on_ir_triggered(char *arg) {
    ir_triggered = true;
}

static void init_task(void) {
    IotUartAttribute uart_attr = {
        .baudRate = 115200,                          // 波特率
        .dataBits = IOT_UART_DATA_BIT_8,             // 数据位
        .stopBits = IOT_UART_STOP_BIT_1,             // 停止位
        .parity   = IOT_UART_PARITY_NONE,            // 奇偶校验
        .rxBlock  = IOT_UART_BLOCK_STATE_NONE_BLOCK, // 接收是否阻塞
        .txBlock  = IOT_UART_BLOCK_STATE_NONE_BLOCK, // 发送是否阻塞
    };

    // 初始化 LED GPIO
    IoTGpioInit(GPIO_LED);
    IoTGpioSetFunc(GPIO_LED, IOT_GPIO_FUNC_GPIO_2_GPIO);
    IoTGpioSetDir(GPIO_LED, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(GPIO_LED, IOT_GPIO_VALUE0);
    // 初始化红外 GPIO
    IoTGpioInit(GPIO_IR);
    IoTGpioSetFunc(GPIO_IR, IOT_GPIO_FUNC_GPIO_11_GPIO);
    IoTGpioSetDir(GPIO_IR, IOT_GPIO_DIR_IN);
    IoTGpioRegisterIsrFunc(GPIO_IR, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_RISE_LEVEL_HIGH,
                            on_ir_triggered, NULL);
    // 初始化舵机 GPIO
    servos[0].pin = GPIO_SERVO0;
    servos[1].pin = GPIO_SERVO1;
    servos[2].pin = GPIO_SERVO2;
    servos[3].pin = GPIO_SERVO3;
    for (int i = 0; i < SERVO_NUM; i++) {
        IoTGpioInit(servos[i].pin);
        IoTGpioSetFunc(servos[i].pin, 0); // GPIO
        IoTGpioSetDir(servos[i].pin, IOT_GPIO_DIR_OUT);
    }
    // 初始化 UART1
    IoTGpioInit(GPIO_UART_TX);
    IoTGpioSetFunc(GPIO_UART_TX, IOT_GPIO_FUNC_GPIO_0_UART1_TXD);
    IoTGpioInit(GPIO_UART_RX);
    IoTGpioSetFunc(GPIO_UART_RX, IOT_GPIO_FUNC_GPIO_1_UART1_RXD);
    IoTUartInit(IOT_UART, &uart_attr);

    // 配置低功耗回调
    hi_lpc_register_hw_handler(hw_prepare, hw_resume);
    // 配置低功耗唤醒 io
    hi_lpc_config_dsleep_wakeup_io(GPIO_IR, HI_TRUE);
    // 设置系统休眠为深睡模式
    hi_lpc_set_type(HI_DEEP_SLEEP);
    // 开启 wifi 低功耗
    hi_wifi_set_pm_switch(HI_TRUE, 0);
}

static void ServoTask(void* param) {
    (void) param;
    init_task();

    uint32_t sleep_timer = 0;
    uint8_t buffer[512];
    while (1) {
        hi_u32 now = hi_get_seconds();
        IotGpioValue ir_val;
        IoTGpioGetInputVal(GPIO_IR, &ir_val);
        // 睡眠计时器未超时或红外传感器被触发
        if (now - sleep_timer < SLEEP_TIMEOUT || ir_val == IOT_GPIO_VALUE1) {
            // 执行舵机逻辑
            for (int i = 0; i < 10; i++) { // 循环内代码执行一次需 20ms
                for (int i = 0; i < SERVO_NUM; i++) {
                    IoTGpioSetOutputVal(servos[i].pin, IOT_GPIO_VALUE1);
                }
                delay_us(1500);
                for (int i = 0; i < SERVO_NUM; i++) {
                    if (now - servos[i].time > LID_OPEN_TIME) {
                        IoTGpioSetOutputVal(servos[i].pin, IOT_GPIO_VALUE0);
                    }
                }
                delay_us(1000);
                for (int i = 0; i < SERVO_NUM; i++) {
                    IoTGpioSetOutputVal(servos[i].pin, IOT_GPIO_VALUE0);
                }
                delay_us(17500);
                // 喂狗, 防止重启
                hi_watchdog_feed();
            }

            // 通过串口 1 接收数据
            int ret = IoTUartRead(IOT_UART, buffer, sizeof(buffer));
            if (ret > 0) {
                now = hi_get_seconds();
                buffer[ret] = '\0';
                printf("Uart1 received: %s\n", buffer);
                for (int i = 0; i < ret; i++) {
                    if (buffer[i] >= '0' && buffer[i] <= '3') {
                        int num = buffer[i] - '0';
                        printf("Received servo command: %d\n", num);
                        servos[num].time = now;
                    }
                }
                sleep_timer = now;
            }
        } else {
            // 通过串口 1 发送数据
            IoTUartWrite(IOT_UART, cmd_sleep, sizeof(cmd_sleep));
            // 尝试深睡直至收到红外传感器上升沿跳变
            IoTGpioSetOutputVal(GPIO_LED, IOT_GPIO_VALUE1);
            for (int i = 0; i < SERVO_NUM; i++) {
                IoTGpioSetOutputVal(servos[i].pin, IOT_GPIO_VALUE0);
            }
            ir_triggered = false;
            while (!ir_triggered) {
                hi_udelay(100000);
                hi_watchdog_feed();
            }
            IoTGpioSetOutputVal(GPIO_LED, IOT_GPIO_VALUE0);
            // 通过串口 1 发送数据
            IoTUartWrite(IOT_UART, cmd_wake, sizeof(cmd_wake));
            // 重置睡眠定时器
            sleep_timer = hi_get_seconds();
        }
    }
}

void SmartTrashBin(void) {
    osThreadAttr_t attr;
    attr.name = "ServoTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 4096;
    attr.priority = osPriorityAboveNormal;
    if (osThreadNew((osThreadFunc_t) ServoTask, NULL, &attr) == NULL) {
        printf("Failed to create ServoTask!\n");
    }
}

APP_FEATURE_INIT(SmartTrashBin);
